Fast nonlinear model predictive planner and control for an unmanned ground vehicle in the presence of disturbances and dynamic obstacles

This paper presents a solution for the tracking control problem, for an unmanned ground vehicle (UGV), under the presence of skid-slip and external disturbances in an environment with static and moving obstacles. To achieve the proposed task, we have used a path-planner which is based on fast nonlinear model predictive control (NMPC); the planner generates feasible trajectories for the kinematic and dynamic controllers to drive the vehicle safely to the goal location. Additionally, the NMPC deals with dynamic and static obstacles in the environment. A kinematic controller (KC) is designed using evolutionary programming (EP), which tunes the gains of the KC. The velocity commands, generated by KC, are then fed to a dynamic controller, which jointly operates with a nonlinear disturbance observer (NDO) to prevent the effects of perturbations. Furthermore, pseudo priority queues (PPQ) based Dijkstra algorithm is combined with NMPC to propose optimal path to perform map-based practical simulation. Finally, simulation based experiments are performed to verify the technique. Results suggest that the proposed method can accurately work, in real-time under limited processing resources.


List of symbols a r
Reference Acceleration q Actual trajectory of the UGV q r Reference trajectory of the UGV q g Initial states of the UGV S(q) Kinematics  Kinematic compensation gain for x k y Kinematic compensation gain for y k θ Kinematic compensation gain for θ k x sub-optimal scaling factor for x k y sub-optimal scaling factor for ŷ k θ sub-optimal scaling factor for θ K f Electromotive force coefficient K c Input saturation's compensation gain W Evolutionary weights P Population vector S(q) Kinematics function of UGV C Centripetal and coriolis matrix C opt Optimal individuals of population P of evolutionary programming In recent years, unmanned ground vehicles (UGVs) have been applied extensively in agriculture, construction, mining and military operations, etc. The UGVs have lightweight, fast speed, low energy consumption, high maneuverability, convenient drive, and simple mechanism 1-3 . Consequently, these advantages make UGVs suitable for indoor 4,5 and outdoor 6,7 applications. However, the ground vehicles can be perturbed due to uncertainties in models, and description of the context of operation 8 , skidding and slipping 9 , noisy sensor's measurements, 10 , and failures in electromechanical components 11 , making the navigation and control of the platform a challenging task 12,13 .
Motion planning and control are the two crucial factors for a vehicle to complete an assigned task safely and effectively 14 . First, it should navigate in an environment with obstacles. Second, it must satisfy the requirement of reaching a specified goal, constantly dealing with perturbations 15 . Thus, the first requirement implied using a path-planner to plan the vehicle's motion, while the second requirement can be treated by a controller to control the kinematics and dynamics of the vehicle.
In the past few decades, research on motion planning and control of UGVs has witnessed remarkable development. In particular, trajectory tracking and target posture stabilization problems on kinematic models were investigated [16][17][18][19] . Additionally, many researchers developed motion control at dynamic levels (such as torque controllers 20,21 or voltage controllers 22,23 ). Moreover, there are some methods developed on model predictive control (MPC), which considers the constraints (such as [24][25][26]. Similarly, many path-planners and path-followers were developed to reach the specified final location safely [27][28][29][30] . From the perspective of UGVs, some studies consider the presence of disturbances, and uncertainties [31][32][33] ; some others assume pure rolling without skid-slip disturbances 34,35 . In addition, some autonomous system use safe switched tracking 36  www.nature.com/scientificreports/ Overall, most of the existing techniques mentioned above assume that the UGVs satisfy pure rolling conditions (without skid-slip). However, pure rolling may not always hold due to the terrain, tire dynamics, voltage drive circuitry, etc. Thus, making the control problem challenging for the UGVs in the presence of external disturbances and skid-slip. Nevertheless, on the other hand, it allows solving these challenges with advanced planning and control strategies. In 38 , kinematic models that are explicitly linked to skid-slip are presented, which considers the design from a controller's perspective. An analysis of the hybrid MPC for stabilization of robot under the presence of wheel slippage is discussed in 39 . In 40 , a nonlinear disturbance observer is used for a selfbalancing mobile wheeled mobile robot. Additionally, this work focuses on a robust tracking control problem with unknown disturbances. In 41 , an NDO and extended Kalman filter (EKF) are combined to perform the trajectory tracking control for wheeled mobile robots. First, a kinematic model excluding perturbation and distorted dynamic model is discussed. Then, NDO is used to observe the external disturbances, and EKF is used to estimate the platform's states. In addition, robust constrained control is developed by observing the disturbances using disturbance observer in 42 .
In terms of path-tracking control problem, an obstacle avoidance mobile robot is introduced in 26 . Firstly, a CasAdi-based 43 single shooting NMPC path-planner is designed. Secondly, a velocity-based virtual control and saturated torque control are designed to apply control actions to the mobile robotic platform. Finally, an extended state observer (ESO) is designed to estimate the disturbances. This technique only considers a single shooting (SS) optimization problem, which requires a larger prediction horizon to compute the NMPC-based path planner. A higher prediction horizon can be understandable in the presence of a complex control problem. However, in the simulations, only a simple point with one dynamic obstacle is avoided. The processing time of the onboard computer is not considered in the simulations, which can cause an issue in the real experiment with the emphasis on the processing time per iteration of the computer. One way to fix the problem is to introduce the multiple shooting(MS) optimization problems, which takes a small prediction horizon. Alternatively, the proximal averaged Newton-type method of optimal control (PANOC) can be used 24 . In terms of voltage control strategies, an adaptive perturbation rejection technique is proposed in 44 . Although they have not considered skid-slip explicitly, the proposed voltage control strategy can solve the trajectory tracking control problem.
This paper focuses on the tracking control problem of a UGV to safely reach a goal location in the presence of moving and static obstacles, skid-slip, and external disturbances. An NMPC-based path-planner, voltage driving control, kinematic and dynamic control, and NDO are used to stabilize the target posture, while PPQ-Dijkstra is used to generate optimal path for the map-based simulation, which is also used in 45 . The main contributions of this research are: (1) In contrast to 26 , the proposed NMPC-based path planner is designed to propose a suitable trajectory to achieve the goal location and avoid the vehicle from colliding with multiple dynamic and static obstacles. Additionally, the MS optimization problem and voltage control strategy are proposed in this paper, which improves the processing time of a single or multicore CPU. (2) A novel EP tuning method is used to tune the gains of the KC to generate velocity commands to the dynamic controller. A dynamic controller and NDO are then proposed to provide voltage-based control actions to the UGV and compensate for the lumped disturbances, respectively. (3) An investigation is performed to measure the single-core and multi-core CPUs processing time per iteration using the proposed MS-based NMPC with the SS method in 26 . In this paper, the proposed control scheme is designed to cope with the dynamic environment. To verify the working of the proposed scheme, extensive simulations have been carried out to show the vehicle's behavior in the presence of disturbances in a realistic framework.

Problem formulation
In this paper we use dynamic and kinematic models which consider disturbances, including skidding and slippage; those models are the following ones 46,47 with, Consider the UGV represented in Fig. 1 is driven by two DC motors attached on each side with same mechanical properties with N > 1 . An expression for the angular velocities of motor could have the following expression: T . Now, we can convert our torque dynamics into the voltage driving circuit by using the above expression as follows:

Methods
In this Section, first, an NMPC-based path planner is to generate reference trajectory to reach the current destination pose, additionally satisfying the collision avoidance constrain. Second, an EP-based gain tuning method is designed to generate KC-based velocity commands. Third, a dynamic control is designed with the NDO to mitigate the effects of disturbances and treat the skid-slip effect. Finally, the stability analysis of the proposed controller is discussed. Figure 2 illustrates the overall close-loop structure of the proposed technique.  www.nature.com/scientificreports/ NMPC-based path planner. we will consider NMPC-based path planner to generate a reference trajectory for the UGV from a starting pose q(0). In particular, the the presences of static and dynamic objects are considered in the environment of the UGV. For the reference posture of the UGV, we consider that the platform operates free of skid-slip, which corresponds to the assumption of pure rolling. Thus, by using the pure rolling, the kinematics and dynamics of the UGV can be expressed as 48 where a r = [v ,ω] T and � r = [v r , ω r ] T . Until now, we have taken we have modelled the platform by a continuous time model. as a continuous-time system. However, it is essential for a computer to generate discrete signals. Therefore, by using Euler's approximation with T s , we can approximate our nonlinear continuous-time system by a discrete-time one, as follows The discrete indexing k ∈ , while is a set of positive natural numbers.
In terms of collision avoidance, the UGV must move with some restrictions. Therefore, we have set constraints on the platform's physical position as follows: where obstacles are i = 1, 2, . . . , I ; I represent number of currently known obstacles. Additionally, constraints on the velocities and accelerations are considered, as follows: Now, we can compute the cost-function for our NMPC-based path planner as follows: The nonlinear problem proposed in (10) and (11) can be transformed into optimal control problem by using the multiple shooting numerical method proposed by 43 . Alternatively, the single shooting method, as proposed in 26 , can be used. However, it would be computationally more complex.
Kinematic controller. The evolutionary strategies narrow the degree of freedom in the instantiating of meta-heuristic algorithms. Thus, the main idea here is first to take the error between the reference and actual positions and orientation. Now, by taking the global transformation mentioned in 49 , we take the following posture error of the real UGV from (12): The time derivative of the tracking error of (13) is as follows Now, a back-stepping method (as that presented in 49 ) is used to get a control signals excluding skid-slip. (1) and k * = [k x ,k y ,k θ ] ∈ R 3 . Now, the main idea here is to find sub-optimal scaling factors k x ,k y and k θ . These sub-optimal scaling factors can be achieved by applying the EP searching algorithm shown in Algorithm 1. . In addition, we can take �τ = (τ (k + 1) − τ (k)) to handle the saturation constraints. This will also gives an intuition that u P is related to unknown skid-slip. Therefore the above expression becomes as follows To accomplish the NDO, we first need to lumped the estimated disturbances as follows where L(q) is the observer gain. Now, we can define the auxiliary variable D to obtain an improved disturbance observer design as follows: where the function p(u p ) is selected from 50 .
where K is the constant matrix. Now, by taking the time derivative of (22) By combining the expressions Eqs. (21) and (22) Thus, we select the observer gain matrix as L(q) = K ·M −1 . Now, using Eqs. (19) and (22) the expression can be re-written as follows: The stability analysis of the observer is performed as per 26 . Therefore, we will discuss the closed loop stability analysis of the controller scheme in the following subsection.

Stability analysis.
To provide the stability analysis of the closed loop system in Fig. 3, we will first define an important assumption as follows: Assumption 1 49 Consider a first order continuous differential variable α 1 (t) ∈ [0, ∞) , which has a limit with t → ∞ . Thus, the second derivative of α(t) does existed and bounded for all t ∈ [0, ∞) such that first derivative holds the condition lim t→∞α1 (t) = 0.
Consider the model of UGV Eqs. (1) and (2), the posture control error Eqs. (17) and (20), the NDO Eqs. (21)- (26), and controllers Eqs. (5) and (27). Then a compensated error-inequality as follows can be refined and we can find error-based asymptotic stability. while, .Consider the following Lyapunov function It is important to notice here that skew symmetry property makes the first term zero, which allows us to rewrite the V 2 as where H max is the maximum NDO estimation error. Now, we can substitute the inequality and the derivatives of the velocity error in Eq. (17), we have Now, we will introduce EPKC from Eq. (22), defining k y = k x k θ v r and substitute in Eq. (32).
In which it can be seen that the first two terms of the above equation are negative, which also guarantees negative V 1 as long as the following is satisfied Therefore, we can write-down the velocity error between EPKC and the estimation velocity as The error functions e s = [e , e c ] T is bounded, therefore, both e and �ė� are bounded. In addition, lim t→∞V1 = 0 can be obtained from the Assumption 1. Thus, it can be concluded that posture and velocity errors of the UGV are asymptotically stable. Simulation 1: Path-tracking in a large map using PPQ-Dijkstra. In this path-tracking experiment, the tracking process is combined with a 2 DoF optimal planner (PPQ-Dijkstra), and considered the University of New South Wale's evacuation map. The map has a size of (1350 m × 600 m) which consists of the buildings of the campus. The intuition behind the experiment is to avoid moving obstacles (MOs) and static obstacles (SOs) in a large map. For this purpose, three destinations are provided as shown in the Fig. 3a, while UGV able to select for selecting the shortest path from the PPQ-Dijkstra (which can deal with multiple destinations). The color lines in the Fig. 3a represents how expensive it is to reach the goal. For example, red has the highest value of 300 which refers to a higher cost, so it is necessary to follow the vehicle to follow blue color (0 value) in order to follow the lowest cost. The NMPC is looking after the non-holonomic constraints of the vehicle as well as the collision avoidance. Figure 3b represents the UGV avoiding the incoming MO, additionally, providing the prediction of the vehicle's state.

Results and discussions
In terms of the performance of the proposed scheme, Fig. 3c represents the velocity profile applied to the real-platform. Clearly, the chattering in both longitudinal velocity and yaw rate represents the collision avoidance in the environment. As far as tracking errors are concerned, Fig. 3d shows that the actual states of the UGV has a good accuracy. Simulation 2: Circle trajectory tracking. For this simulation experiment, we have considered the following expression to form a circle: (31) V 1 = 2k 1 (e x + l(1 − cos(e θ ))(ė x + l sin(e θ )ė θ ) + 2k 1 (e y − l sin(e θ )(ė θ − l cos(e θ ) + 2k e v r sin(e θ )ė θ +V 2 (32) The intention for this simulation is to avoid collisions and follow the reference trajectory. For first collision avoidance, the UGV predicts SO and MO to divert from the reference trajectory as shown in the Fig. 4a. Since, the longitudinal velocity of the vehicle is 1 m/s , it increases speed to avoid the MO safely as shown in the Fig. 4b. In particular, this is an example of a vehicle over taking another vehicle in the environment. The UGV tracks back on the (38) x r (t) = 5sin(0.2t),  Fig. 4d. In Fig. 4e the platform properly continues the trajectory tracking process, for finally dealing with a second incoming DO and an additional SO, both requiring an avoidance treatment, which is successfully performed, for finally completing the required trajectory as shown in Fig. 4f. In terms of meeting the input constraints imposed by the NMPC, Fig. 5 illustrates the applied velocities and accelerations to the UGV. The reference velocity is considered as 0.7m/s, which the vehicles tries to match. However, since the environment is filled with obstacles, it can only reach the reference velocity once the obstacles are avoided as shown in the Fig. 5a. Additionally, the longitudinal velocity increases in the time instances 10(sec) − 15(sec) , which indicates avoidance of obstacles safely and then converging to the reference velocity. For the angular velocity, a similar trend can be observed in the Fig. 5b. For the rate of change in the longitudinal and angular velocities in Fig. 5c,d, chattering are visible in the signal. However, it always converges to zero once it meets the reference trajectories. The reason for having chattering are due to the continuous occurrence of multiple obstacles, which diverts the UGV from it's nominal trajectory.
In terms of trajectory tracking error, we have computed a case with no obstacles and compared it with the one, which we have simulated in this experiment as shown in the Fig. 6a. The mean error for both situations are respectable. However, it is appreciable that without obstacles the tracking error is very small, while the error with obstacles are still having reasonable values. Additionally, for control action applied to the UGV, the applied voltages are converging to a steady-state as shown in the Fig. 6b, which proves that motor would work adequately despite these challenging conditions of obstacles and skid-slip.
Performance evaluation of the proposed method. To validate the robustness of the proposed method, we have evaluated our method with 26 . The comparison is performed as per the error accuracy, capability of the NMPC to run on a single core and multiple cores CPU, and average processing time of the NMPC to compute a single optimization iteration. As mentioned earlier in the paper, our numerical method is based on multiple shooting problem, which is tested in different prediction horizon of time.
The ability to perform any optimization method on an on-board processor must be feasible. For instance, a single-core CPU with limited resources must be able to solve the optimization problem effectively. For this comparison, Table 1 discusses the proposed method utilizes the average processing time per iteration (PT/I) for this paper (using MS) and compared with SS. In terms of computing NMPC on single or multi-core CPUs, www.nature.com/scientificreports/ the average PT/I in this paper is relatively better than SS method. In addition, even if we increase the prediction horizon to a larger number N p = 150 , the PT/I always feasible to run this on the CPU with limited resources. This also enables to perform a real experiment with an on board processor with limited processing capabilities. In contrast to 26 , a remarkable finding is that accuracy in the MS method is optimal for a small and a large prediction horizon. Although, an increase in the prediction horizons may cause a slower processing but a lower error as shown in the Table 2. However, the issue with increasing a too large prediction horizon isn't feasible for some cases. For instance, in 26 , a prediction horizon of N p = 100 was used to run the path planner, which has an accuracy of 2cm. However, this can lead to a larger processing time. Thus, based on these findings, the proposed method is reasonable for real-time applications.
Skid-slip analysis. In terms of including the skid-slip applied to simulations, we have also used the PWLbased function proposed in Fig. 7a for the circle trajectory simulation. Furthermore, the side-slip angle of front wheels is within a reasonable range (slip angle) β ≤ |10deg| as shown in Fig. 7b,c.

Conclusions
In this paper, we propose a tracking control and planner for a UGV in the presence of a skid-slip and dynamic and static objects. The proposed technique consists of a fast NMPC-based path-planner, novel evolutionary programming-based kinematic controller, a dynamic controller with a nonlinear disturbance observer, and a voltage driving circuit. We have performed simulation experiments by considering actual physical parameters to drive the vehicle safely to the goal location. First, a large map of UNSW is used, which is fed to PPQ-Dijkstra algorithm, and NMPC is combined with the map to avoid collisions and non-holonomic constraints. Finally, a test in which the UGV must follow a sizeable circular trajectory under the presence of static and moving objects is performed. The results obtained from these experiments validate that the proposed method can safely drive the vehicle to the goal location. To validate the performance of our proposed technique, we have performed